/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.domain;

import SSMPC2Java.SSMPC2Java;
import br.ufrn.engcomp.mpc.util.MATLABUtils;
import br.ufrn.engcomp.mpc.util.MathCommonsUtils;
import com.mathworks.toolbox.javabuilder.MWClassID;
import com.mathworks.toolbox.javabuilder.MWComplexity;
import com.mathworks.toolbox.javabuilder.MWException;
import com.mathworks.toolbox.javabuilder.MWNumericArray;
import java.util.logging.Level;
import java.util.logging.Logger;
import org.apache.commons.math.linear.*;

/**
 *
 * @author Felipe
 */
public class HybridSSPredictiveController extends AbstractPredictiveController {
    
    private SSMPC2Java matlabSSMPC;
    
    private RealMatrix psiMatrix;
    private RealMatrix gammaMatrix;
    private RealMatrix thetaMatrix;
    private RealMatrix powersAMatrix; // Utilizada para otimizar o cálculo de PSI
    private RealMatrix powersABMatrix; // Utilizada para otimizar o cálculo de GAMMA
    private RealMatrix multiColumnPowersABMatrix; // Utilizada para otimizar o cálculo de THETA
    private RealMatrix identityCMatrix;
    private RealMatrix qMatrix; // Termo de ponderação do erro
    private RealMatrix rMatrix; // Termo de ponderação da variação do sinal de controle
    private RealMatrix hMatrix;
    private RealMatrix apostropheGMatrix;
    private RealMatrix eConstraintsMatrix;
    private RealMatrix fConstraintsMatrix;
    private RealMatrix gConstraintsMatrix;

    public HybridSSPredictiveController(Plant plant) {
        super(plant);
    }
    
    @Override
    protected double[] synchComputeU(double[] setPoint, double[] lastX, double[] lastY, double[] lastU, double[] lastStep) {
        double[] u = new double[lastU.length];
        int cB = plant.getBMatrix().getColumnDimension();
        int rC = plant.getCMatrix().getRowDimension();
        
        try {
            MWNumericArray setPointVector = generateVector(setPoint);
            MWNumericArray lastXVector = generateVector(lastX);
            MWNumericArray lastYVector = generateVector(lastY);
            MWNumericArray lastUVector = generateVector(lastU);
            
            MWNumericArray matlabPsiMatrix = MATLABUtils.convertMatrixJavaToMATLAB(psiMatrix);
            MWNumericArray matlabGammaMatrix = MATLABUtils.convertMatrixJavaToMATLAB(gammaMatrix);
            MWNumericArray matlabHMatrix = MATLABUtils.convertMatrixJavaToMATLAB(hMatrix);
            MWNumericArray matlabApostropheGMatrix = MATLABUtils.convertMatrixJavaToMATLAB(apostropheGMatrix);
            MWNumericArray matlabPowersAMatrix = MATLABUtils.convertMatrixJavaToMATLAB(powersAMatrix);
            MWNumericArray matlabPowersABMatrix = MATLABUtils.convertMatrixJavaToMATLAB(powersABMatrix);
            MWNumericArray matlabMultiColumnPowersABMatrix = MATLABUtils.convertMatrixJavaToMATLAB(multiColumnPowersABMatrix);
            
            Object[] result;
            
            if (plant.isConstrained()) {
                generateConstraints(new ArrayRealVector(setPoint), new ArrayRealVector(lastX), new ArrayRealVector(lastStep));
            
                MWNumericArray matlabQMatrix = MATLABUtils.convertMatrixJavaToMATLAB(qMatrix);
                
                MWNumericArray matlabEConstraintsMatrix = MATLABUtils.convertMatrixJavaToMATLAB(eConstraintsMatrix);
                MWNumericArray matlabFConstraintsMatrix = MATLABUtils.convertMatrixJavaToMATLAB(fConstraintsMatrix);
                MWNumericArray matlabGConstraintsMatrix = MATLABUtils.convertMatrixJavaToMATLAB(gConstraintsMatrix);
                
                result = matlabSSMPC.constrainedSsmpc2Java(1, hp, hu, cB, rC, matlabQMatrix, setPointVector, lastXVector, lastYVector, lastUVector, matlabPsiMatrix, matlabGammaMatrix, matlabHMatrix, matlabApostropheGMatrix, matlabPowersAMatrix, matlabPowersABMatrix, matlabMultiColumnPowersABMatrix, matlabEConstraintsMatrix, matlabFConstraintsMatrix, matlabGConstraintsMatrix);
            } else {
                result = matlabSSMPC.ssmpc2Java(1, hp, hu, cB, rC, setPointVector, lastXVector, lastYVector, lastUVector, matlabPsiMatrix, matlabGammaMatrix, matlabHMatrix, matlabApostropheGMatrix);
            }
            
            u = ((MWNumericArray) result[0]).getDoubleData();
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
        
        return u;
    }
    
    // NOVO
    @Override
    protected double[] synchComputeU(double[] setPoint, double[] lastX, double[] lastY, double[] lastU, double[] lastStep, double d) {
        double[] u = new double[lastU.length];
        int cB = plant.getBMatrix().getColumnDimension();
        int rC = plant.getCMatrix().getRowDimension();
        
        try {
            MWNumericArray lastXVector = generateVector(lastX);
            
            Object[] result = matlabSSMPC.ssmpc2Java(1, hp, hu, cB, rC, setPoint, lastXVector, lastY, lastU, psiMatrix, gammaMatrix, hMatrix, apostropheGMatrix);
            
            u = ((MWNumericArray) result[0]).getDoubleData();
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
        
        return u;
    }

    @Override
    protected final void initEnviroment() {
        initMATLAB();
    }

    private void initMATLAB() {
        try {
            matlabSSMPC = new SSMPC2Java();
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    @Override
    protected void initStaticData() {
        computePowersAMatrix();
        computePowersABMatrix();
        computeMultiColumnPowersABMatrix();
        computeIdentityCMatrix();
        computePsiMatrix();
        computeGammaMatrix();
        computeThetaMatrix();
        generateQMatrix();
        generateRMatrix();
        computeHMatrix();
        computeApostropheGMatrix();
//        generateConstrains();
    }

    private void computePowersAMatrix() {
        int oA = plant.getAMatrix().getRowDimension();
        powersAMatrix = MathCommonsUtils.zeros(hp * oA, oA);
        RealMatrix powerA = plant.getAMatrix().copy();

        powersAMatrix.setSubMatrix(powerA.getData(), 0, 0);

        int startPointer;

        for (int i = 1; i < hp; i++) {
            startPointer = MathCommonsUtils.getStartPointer(i, oA);

            powerA = powerA.multiply(plant.getAMatrix());

            powersAMatrix.setSubMatrix(powerA.getData(), startPointer, 0);
        }
    }

    private void computePowersABMatrix() {
        int oA = plant.getAMatrix().getRowDimension();
        int cB = plant.getBMatrix().getColumnDimension();
        RealMatrix sum = plant.getBMatrix().copy();
        powersABMatrix = MathCommonsUtils.zeros(hp * oA, cB);

        powersABMatrix.setSubMatrix(sum.getData(), 0, 0);

        int startPointer;
        int sumStartPointer;
        int sumEndPointer;

        for (int i = 1; i < hp; i++) {
            startPointer = MathCommonsUtils.getStartPointer(i, oA);
            sumStartPointer = MathCommonsUtils.getStartPointer(i - 1, oA);
            sumEndPointer = MathCommonsUtils.getEndPointer(sumStartPointer, oA);

            sum = sum.add(powersAMatrix.getSubMatrix(sumStartPointer, sumEndPointer, 0, powersAMatrix.getColumnDimension() - 1).multiply(plant.getBMatrix()));

            powersABMatrix.setSubMatrix(sum.getData(), startPointer, 0);
        }
    }

    private void computeMultiColumnPowersABMatrix() {
        int oA = plant.getAMatrix().getRowDimension();
        int cB = plant.getBMatrix().getColumnDimension();
        multiColumnPowersABMatrix = MathCommonsUtils.zeros(hp * oA, hu * cB);
        RealMatrix zeroColumn;
        RealMatrix gammaColumn;
        RealMatrix concMatrix;
        int aux = 0;

        int cStartPointer;

        for (int i = 0; i < hu; i++) {
            cStartPointer = MathCommonsUtils.getStartPointer(i, cB);

            zeroColumn = MathCommonsUtils.zeros(aux * oA, cB);
            gammaColumn = powersABMatrix.getSubMatrix(0, powersABMatrix.getRowDimension() - 1 - aux * oA, 0, powersABMatrix.getColumnDimension() - 1);

            concMatrix = MathCommonsUtils.concatenateRow(zeroColumn, gammaColumn);

            multiColumnPowersABMatrix.setSubMatrix(concMatrix.getData(), 0, cStartPointer);

            aux++;
        }
    }

    private void computeIdentityCMatrix() {
        identityCMatrix = MathCommonsUtils.generateIdentityFromMatrix(plant.getCMatrix(), hp);
    }

    private void computePsiMatrix() {
        psiMatrix = identityCMatrix.multiply(powersAMatrix);
    }

    private void computeGammaMatrix() {
        gammaMatrix = identityCMatrix.multiply(powersABMatrix);
    }

    private void computeThetaMatrix() {
        thetaMatrix = identityCMatrix.multiply(multiColumnPowersABMatrix);
    }

    private void generateQMatrix() {
        int rC = plant.getCMatrix().getRowDimension();
        RealMatrix subMatrix = new Array2DRowRealMatrix(rC, rC);
        qMatrix = MathCommonsUtils.eye(rC * hp);

        for (int i = 0; i < rC; i++) {
            subMatrix.setEntry(i, i, q[i]);
        }

        int startPointer;

        for (int i = (hw - 1); i < hp; i++) {
            startPointer = MathCommonsUtils.getStartPointer(i, rC);

            qMatrix.setSubMatrix(subMatrix.getData(), startPointer, startPointer);
        }
    }

    private void generateRMatrix() {
        int cB = plant.getBMatrix().getColumnDimension();
        RealMatrix subMatrix = new Array2DRowRealMatrix(cB, cB);
        rMatrix = MathCommonsUtils.eye(cB * hu);

        for (int i = 0; i < cB; i++) {
            subMatrix.setEntry(i, i, r[i]);
        }

        int startPointer;

        for (int i = 0; i < hu; i++) {
            startPointer = MathCommonsUtils.getStartPointer(i, cB);

            rMatrix.setSubMatrix(subMatrix.getData(), startPointer, startPointer);
        }
    }

    // H = THETA' * Q * THETA + R
    private void computeHMatrix() {
        hMatrix = thetaMatrix.transpose().multiply(qMatrix).multiply(thetaMatrix).add(rMatrix);
    }

    // G' = 2 * THETA' * Q * THETA'
    private void computeApostropheGMatrix() {
        apostropheGMatrix = thetaMatrix.transpose().scalarMultiply(2).multiply(qMatrix);
    }

//    private void generateConstrains() {
//        if (plant.isConstrained()) {
//            RealMatrix eConstraintsMatrixSeed = plant.getConstraints().getEConstraintsMatrixSeed();
//            RealMatrix fConstraintsMatrixSeed = plant.getConstraints().getFConstraintsMatrixSeed();
//            RealMatrix gConstraintsMatrixSeed = plant.getConstraints().getGConstraintsMatrixSeed();
//
//            if (eConstraintsMatrixSeed != null) {
//                eConstraintsMatrix = generateConstraintsMatrixFromSeed(eConstraintsMatrixSeed, hu);
//            }
//
//            if (fConstraintsMatrixSeed != null) {
//                fConstraintsMatrix = generateConstraintsMatrixFromSeed(fConstraintsMatrixSeed, hu);
//            }
//
//            if (gConstraintsMatrixSeed != null) {
//                gConstraintsMatrix = generateConstraintsMatrixFromSeed(gConstraintsMatrixSeed, hp);
//            }
//        }
//    }

    private void generateConstraints(RealVector setPointVector, RealVector lastXVector, RealVector lastStepVector) {
        PlantWithConstraints plantWithConstraints = (PlantWithConstraints) plant;
        
        RealMatrix staticEConstraintsMatrixSeed = plantWithConstraints.getStaticEConstraintsMatrixSeed();
        RealMatrix staticFConstraintsMatrixSeed = plantWithConstraints.getStaticFConstraintsMatrixSeed();
        RealMatrix staticGConstraintsMatrixSeed = plantWithConstraints.getStaticGConstraintsMatrixSeed();
        RealMatrix dynamicEConstraintsMatrixSeed = plantWithConstraints.getDynamicEConstraintsMatrixSeed();
        RealMatrix dynamicFConstraintsMatrixSeed = plantWithConstraints.getDynamicFConstraintsMatrixSeed(lastXVector);
        RealMatrix dynamicGConstraintsMatrixSeed = plantWithConstraints.getDynamicGConstraintsMatrixSeed(lastStepVector, setPointVector);
        RealMatrix allEConstraintsMatrixSeed = MathCommonsUtils.concatenateRow(staticEConstraintsMatrixSeed, dynamicEConstraintsMatrixSeed);
        RealMatrix allFConstraintsMatrixSeed = MathCommonsUtils.concatenateRow(staticFConstraintsMatrixSeed, dynamicFConstraintsMatrixSeed);
        RealMatrix allGConstraintsMatrixSeed = MathCommonsUtils.concatenateRow(staticGConstraintsMatrixSeed, dynamicGConstraintsMatrixSeed);

        if (allEConstraintsMatrixSeed != null) {
            eConstraintsMatrix = generateConstraintsMatrixFromSeed(allEConstraintsMatrixSeed, hu);
        }

        if (allFConstraintsMatrixSeed != null) {
            fConstraintsMatrix = generateConstraintsMatrixFromSeed(allFConstraintsMatrixSeed, hu);
        }

        if (allGConstraintsMatrixSeed != null) {
            gConstraintsMatrix = generateConstraintsMatrixFromSeed(allGConstraintsMatrixSeed, hp);
        }
    }

    private RealMatrix generateConstraintsMatrixFromSeed(RealMatrix seedMatrix, int horizon) {
        RealMatrix upperCaseSeedMatrix = seedMatrix.getSubMatrix(0, seedMatrix.getRowDimension() - 1, 0, seedMatrix.getColumnDimension() - 2);
        RealVector lowerCaseSeedVector = seedMatrix.getColumnVector(seedMatrix.getColumnDimension() - 1);
        RealMatrix upperCaseConstraintMatrix = MathCommonsUtils.generateIdentityFromMatrix(upperCaseSeedMatrix, horizon);
        RealMatrix lowerCaseConstraintMatrix = MathCommonsUtils.generateColumnFromVector(lowerCaseSeedVector, horizon);
        
        return MathCommonsUtils.concatenateColumn(upperCaseConstraintMatrix, lowerCaseConstraintMatrix);
    }

    private RealVector generateTauVector(RealVector lastYVector, RealVector setPointVector) {
        int rC = plant.getCMatrix().getRowDimension();
        RealVector tauVector = MathCommonsUtils.convertMatrixToColumnVector(MathCommonsUtils.zeros(hp * rC, 1));
        RealVector onesVector = MathCommonsUtils.convertMatrixToColumnVector(MathCommonsUtils.ones(hp, 1));
        
        int startPointer;

        for (int i = 0; i < rC; i++) {
            startPointer = MathCommonsUtils.getStartPointer(i, hp);

            tauVector.setSubVector(startPointer, onesVector.mapMultiply(setPointVector.getEntry(i)));
        }

        return tauVector;
    }

    // EPSILON = TAU(k) - PSI(k) - GAMMA * u(k - 1)
    private RealVector computeEpsilonVector(RealVector tauVector, RealMatrix psiMatrix, RealMatrix gammaMatrix, RealVector lastXVector, RealVector lastUVector) {
        RealMatrix lastXVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(lastXVector);
        RealMatrix lastUVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(lastUVector);

        // PSI * x(k)
        RealVector secondTerm = MathCommonsUtils.convertMatrixToColumnVector(psiMatrix.multiply(lastXVectorAsMatrix));
        // GAMMA * u(k - 1)
        RealVector thirdTerm = MathCommonsUtils.convertMatrixToColumnVector(gammaMatrix.multiply(lastUVectorAsMatrix));

        return tauVector.subtract(secondTerm).subtract(thirdTerm);
    }

    // EPISILON = TAU(k) - PSI(k) - GAMMA * u(k - 1) - 1d(k)
    // onde d(k) = y(k) - y_est(k) e 1d(k) = [1 1 ... 1]' * d(k)
    private RealVector computeEpsilonVector(RealVector tauVector, RealMatrix psiMatrix, RealMatrix gammaMatrix, RealVector lastXVector, RealVector lastUVector, double d) {
        RealMatrix lastXVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(lastXVector);
        RealMatrix lastUVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(lastUVector);

        // PSI * x(k)
        RealVector secondTerm = MathCommonsUtils.convertMatrixToColumnVector(psiMatrix.multiply(lastXVectorAsMatrix));
        // GAMMA * u(k - 1)
        RealVector thirdTerm = MathCommonsUtils.convertMatrixToColumnVector(gammaMatrix.multiply(lastUVectorAsMatrix));
        // d (diferenca entre saída da planta e saída do modelo
        RealVector fourthTerm = MathCommonsUtils.convertMatrixToColumnVector(MathCommonsUtils.ones(hp, 1).scalarMultiply(d));

        return tauVector.subtract(secondTerm).subtract(thirdTerm).subtract(fourthTerm);
    }

    // G = 2 * THETA' Q * THETA * EPSILON
    private RealVector calculateGVector(RealMatrix apostropheGMatrix, RealVector epsilonVector) {
        RealMatrix epsilonVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(epsilonVector);

        return MathCommonsUtils.convertMatrixToColumnVector(apostropheGMatrix.multiply(epsilonVectorAsMatrix));
    }

    private RealMatrix computeAConstraintMatrix(RealMatrix eConstraintsMatrix, RealMatrix fConstraintsMatrix, RealMatrix gConstraintsMatrix, RealMatrix multiColumnPowersABMatrix) {
        RealMatrix inputSlewRateConstraints = null;
        RealMatrix inputConstraints = null;
        RealMatrix outputConstraints = null;
        
        if (eConstraintsMatrix != null) {
            RealMatrix wConstraintMatrix = eConstraintsMatrix.getSubMatrix(0, eConstraintsMatrix.getRowDimension() - 1, 0, eConstraintsMatrix.getColumnDimension() - 2);
            
            inputSlewRateConstraints = wConstraintMatrix;
        }
        
        if (fConstraintsMatrix != null) {
            RealMatrix fancyFConstraintMatrix = generateFancyFConstraintsMatrix(fConstraintsMatrix, hu);

            inputConstraints = fancyFConstraintMatrix;
        }
        
        if (gConstraintsMatrix != null) {
            RealMatrix gammaConstraintMatrix = gConstraintsMatrix.getSubMatrix(0, gConstraintsMatrix.getRowDimension() - 1, 0, gConstraintsMatrix.getColumnDimension() - 2);

            outputConstraints = gammaConstraintMatrix.multiply(multiColumnPowersABMatrix);
        }
        
        return MathCommonsUtils.concatenateRow(inputSlewRateConstraints, inputConstraints, outputConstraints);
    }

    private RealVector computeBConstraintVector(RealMatrix eConstraintsMatrix, RealMatrix fConstraintsMatrix, RealMatrix gConstraintsMatrix, RealMatrix powersAMatrix, RealMatrix powersABMatrix, RealVector lastXVector, RealVector lastUVector) {
        RealMatrix inputSlewRateConstraints = null;
        RealMatrix inputConstraints = null;
        RealMatrix outputConstraints = null;
        
        if (eConstraintsMatrix != null) {
            RealVector lowerCaseWConstraintVector = eConstraintsMatrix.getColumnVector(eConstraintsMatrix.getColumnDimension() - 1);
            
            inputSlewRateConstraints = MathCommonsUtils.convertColumnVectorToMatrix(lowerCaseWConstraintVector);
        }
        
        if (fConstraintsMatrix != null) {
            RealMatrix firstTermFancyFConstraintMatrix = generateFancyFConstraintsMatrix(fConstraintsMatrix, 1);
            RealVector lowerCaseFConstraintVector = fConstraintsMatrix.getColumnVector(fConstraintsMatrix.getColumnDimension() - 1);
            
            inputConstraints = firstTermFancyFConstraintMatrix.scalarMultiply(-1).multiply(MathCommonsUtils.convertColumnVectorToMatrix(lastUVector)).subtract(MathCommonsUtils.convertColumnVectorToMatrix(lowerCaseFConstraintVector));
        }
        
        if (gConstraintsMatrix != null) {
            RealMatrix gammaConstraintMatrix = gConstraintsMatrix.getSubMatrix(0, gConstraintsMatrix.getRowDimension() - 1, 0, gConstraintsMatrix.getColumnDimension() - 2);
            RealVector lowerCaseGConstraintVector = gConstraintsMatrix.getColumnVector(gConstraintsMatrix.getColumnDimension() - 1);
            
            outputConstraints = gammaConstraintMatrix.scalarMultiply(-1).multiply(powersAMatrix.multiply(MathCommonsUtils.convertColumnVectorToMatrix(lastXVector)).add(powersABMatrix.multiply(MathCommonsUtils.convertColumnVectorToMatrix(lastUVector)))).subtract(MathCommonsUtils.convertColumnVectorToMatrix(lowerCaseGConstraintVector));
        }
        
        return MathCommonsUtils.convertMatrixToColumnVector(MathCommonsUtils.concatenateRow(inputSlewRateConstraints, inputConstraints, outputConstraints));
    }

    private RealMatrix generateFancyFConstraintsMatrix(RealMatrix fConstraintsMatrix, int horizon) {
        RealMatrix fancyFConstraintsMatrix = new Array2DRowRealMatrix(fConstraintsMatrix.getRowDimension(), nInputs * horizon);
        RealMatrix fancyFConstraintsMatrixElement = new Array2DRowRealMatrix(fConstraintsMatrix.getRowDimension(), nInputs);
        RealMatrix fConstraintsMatrixElement;
        
        int startPointer;
        int endPointer;
        
        for (int i=hu; i > 0; i--) {
            startPointer = MathCommonsUtils.getStartPointer(i - 1, nInputs);
            endPointer = MathCommonsUtils.getEndPointer(startPointer, nInputs);
            
            fConstraintsMatrixElement = fConstraintsMatrix.getSubMatrix(0, fConstraintsMatrix.getRowDimension() - 1, startPointer, endPointer);
            
            fancyFConstraintsMatrixElement = fancyFConstraintsMatrixElement.add(fConstraintsMatrixElement);
            
            if (i <= horizon) {
                fancyFConstraintsMatrix.setSubMatrix(fancyFConstraintsMatrixElement.getData(), 0, startPointer);
            }
        }
        
        return fancyFConstraintsMatrix;
    }

    // deltaU(k) = (1/2) * inv(H) * G
    private RealVector computeDeltaUVector(RealMatrix hMatrix, RealVector gVector) {
        int cB = plant.getBMatrix().getColumnDimension();
        RealMatrix inverseHMatrix = new LUDecompositionImpl(hMatrix).getSolver().getInverse();
        RealMatrix gVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(gVector);
        RealVector fullDeltaUVector = MathCommonsUtils.convertMatrixToColumnVector(inverseHMatrix.scalarMultiply(0.5).multiply(gVectorAsMatrix));
        RealMatrix firstRowsMatrix = MathCommonsUtils.concatenateColumn(MathCommonsUtils.eye(cB), MathCommonsUtils.zeros(cB, cB * (hu - 1)));
        RealMatrix fullDeltaUVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(fullDeltaUVector);

        return MathCommonsUtils.convertMatrixToColumnVector(firstRowsMatrix.multiply(fullDeltaUVectorAsMatrix));
    }

    private RealVector computeDeltaUVector(RealMatrix hMatrix, RealVector gVector, RealVector epsilonVector, RealMatrix qMatrix, RealMatrix aConstraintMatrix, RealVector bConstraintVector) {
        int cB = plant.getBMatrix().getColumnDimension();
        RealMatrix epsilonVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(epsilonVector);
        double constant = epsilonVectorAsMatrix.transpose().multiply(qMatrix).multiply(epsilonVectorAsMatrix).getEntry(0, 0);
        
        QPSolver solver = new JOptimizerQPSolver();
        
        RealVector fullDeltaUVector = solver.solve(hMatrix.scalarMultiply(2), gVector.mapMultiply(-1), constant, aConstraintMatrix, bConstraintVector);
        RealMatrix firstRowsMatrix = MathCommonsUtils.concatenateColumn(MathCommonsUtils.eye(cB), MathCommonsUtils.zeros(cB, cB * (hu - 1)));
        RealMatrix fullDeltaUVectorAsMatrix = MathCommonsUtils.convertColumnVectorToMatrix(fullDeltaUVector);

        return MathCommonsUtils.convertMatrixToColumnVector(firstRowsMatrix.multiply(fullDeltaUVectorAsMatrix));
    }

    private MWNumericArray generateVector(double[] dataArray) {
        MWNumericArray vector = MWNumericArray.newInstance(new int[] { dataArray.length, 1 }, MWClassID.DOUBLE, MWComplexity.REAL);
        
        for (int i = 0; i < dataArray.length; i++) {
            vector.set(new int[] { i + 1, 1 }, dataArray[i]);
        }
        
        return vector;
    }
    
    private MWNumericArray generateConstraints(int horizon, RealMatrix constraintsSeed) {
        try {
            Object[] result = matlabSSMPC.generateConstraints(1, horizon, MATLABUtils.convertMatrixJavaToMATLAB(constraintsSeed));

        return (MWNumericArray) result[0];
        } catch (MWException ex) {
            Logger.getLogger(MATLABSSPredictiveController.class.getName()).log(Level.SEVERE, null, ex);
        }
        
        return MWNumericArray.newInstance(new int[] { 0, 0 }, MWClassID.DOUBLE, MWComplexity.REAL);
    }
    
}
